/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "Marekbot.h"
#include "Marekbot_settings.h"
#include "owndebug.h"
#include "World.h"

CObject* CMarekbot::LoadMLMA(CWorld *world, xmlNode *node)
{
  CObject *object=NULL;
  // Parameters for a Line.
  xmlChar *x = xmlGetProp(node, (xmlChar *) "x");
  xmlChar *y = xmlGetProp(node, (xmlChar *) "y");
  xmlChar *heading = xmlGetProp(node, (xmlChar *) "heading");
  xmlChar *name = xmlGetProp(node, (xmlChar *) "name");
  xmlChar *group = xmlGetProp(node, (xmlChar *) "MaCIGroup");

  // Process.
  if (x && y && heading && (name || group)) {
    object=new CMarekbot(world,
                         atof((const char*)x),
                         atof((const char*)y),
                         atof((const char*)heading),
                         name!=NULL ? (const char*)name : "",
                         group!=NULL ? (const char*)group : "");
  } else {
    dPrint(1,"MLMA Error:Not enough parameters defined for AGV-object!");
  }

  // Cleanup (Always do this to clean out properties which were found)
  if (x) xmlFree(x);
  if (y) xmlFree(y);
  if (heading) xmlFree(heading);
  if (name) xmlFree(name);
  if (group) xmlFree(group);
  
  if (object)
  {
    world->InsertObject(object);
  }
  
  return object;
}

CMarekbot::CMarekbot(CWorld* World, float x, float y, float heading, std::string name, 
                      std::string aMaCIGroup)
  : CObject(World,NULL),
    name(name),
    MaCIGroup(World->GetBaseMaCIGroup()+"."+aMaCIGroup),
    gimi(),
    mcs(&gimi),
    SpeedCtrlServer(&mcs,0),
    PositionServer(&mcs,0),
    iSpeed(0.0),
    iAngularSpeed(0.0),
    positionTimer(0)
{
  
  if (MaCIGroup.length())
  {
    mcs.SetGroupName(MaCIGroup);
  } else {
    mcs.SetGroupName(name);
  }
  
  if (name.length())
  {
    
  } else {
    name=MaCI::GetStaticClientName(MaCIGroup+"FSRSimSkywalker");
  }
  
  SpeedCtrlServer.SetInterfaceInstanceName("Motion");
  PositionServer.SetInterfaceInstanceName("Motion");
  
#warning "NOTE: Using inverted y-axis and heading"
  y=-y;
  heading=-heading;

  //materials
  WheelMaterial.look.color[0]=0.7;WheelMaterial.look.color[1]=0.7;WheelMaterial.look.color[2]=0.2;

  //WheelMaterial.surface.soft_erp=0.0;
  //WheelMaterial.surface.soft_cfm=0.0;

  HullMaterial.look.color[0]=0.4;HullMaterial.look.color[1]=0.4;HullMaterial.look.color[2]=0.4;
  HullMaterial.density=HULL_MASS/(HULL_WIDTH*HULL_HEIGHT*HULL_LENGTH);


  //objects
  //primitives
  Parts.push_back(Hull=new CBox(HULL_WIDTH,HULL_HEIGHT,HULL_LENGTH,this,SpaceID,0,&HullMaterial));
  Parts.push_back(RWheel=new CCylinder(DRIVING_WHEEL_RADIUS,DRIVING_WHEEL_WIDTH,this,this->SpaceID,0,&WheelMaterial,false));
  Parts.push_back(LWheel=new CCylinder(DRIVING_WHEEL_RADIUS,DRIVING_WHEEL_WIDTH,this,this->SpaceID,0,&WheelMaterial,false));
  Parts.push_back(Caster=new CSphere(CASTER_WHEEL_RADIUS,this,this->SpaceID,0,&WheelMaterial,false));

  //placement and rotation of parts
  dMatrix3 R;
  const dReal *P,*RR;

  dRFromAxisAndAngle(R,0,1,0,DegToRad(-heading-90));
  dBodySetRotation(Hull->BodyID,R);
  dBodySetPosition(Hull->BodyID,x,HULL_Y,y);

  dBodySetPosition(RWheel->BodyID, x+cos(DegToRad(heading+90))*DRIVING_WHEEL_X+
                  sin(DegToRad(heading+90))*DRIVING_WHEEL_Z,
                  DRIVING_WHEEL_RADIUS,
                  y+sin(DegToRad(heading+90))*DRIVING_WHEEL_X-
                  cos(DegToRad(heading+90))*DRIVING_WHEEL_Z);

  dBodySetPosition(LWheel->BodyID, x-cos(DegToRad(heading+90))*DRIVING_WHEEL_X+
                  sin(DegToRad(heading+90))*DRIVING_WHEEL_Z,
                  DRIVING_WHEEL_RADIUS,
                  y-sin(DegToRad(heading+90))*DRIVING_WHEEL_X-
                  cos(DegToRad(heading+90))*DRIVING_WHEEL_Z);

  dRSetIdentity(R);
  dRFromAxisAndAngle(R,0,1,0,-DegToRad(heading));
  dBodySetRotation(RWheel->BodyID,R);
  dBodySetRotation(LWheel->BodyID,R);

  RR=dBodyGetRotation(LWheel->BodyID);

  dBodySetPosition(Caster->BodyID, x+cos(DegToRad(heading))*CASTER_WHEEL_Z,
  CASTER_WHEEL_RADIUS+CASTER_WHEEL_Y, y+sin(DegToRad(heading))*CASTER_WHEEL_Z);

  //joints

  CasterJoint=dJointCreateBall(this->WorldID,0);
  LMotor=dJointCreateHinge(this->WorldID,0);
  RMotor=dJointCreateHinge(this->WorldID,0);
  //dPrint(1,"RMotor=%d, LMotor=%d",RMotor,LMotor);
  dJointAttach(CasterJoint,Hull->BodyID,Caster->BodyID);
  dJointAttach(LMotor,Hull->BodyID,LWheel->BodyID);
  dJointAttach(RMotor,Hull->BodyID,RWheel->BodyID);

  P=dBodyGetPosition(Caster->BodyID);
  dJointSetBallAnchor(CasterJoint,P[0],P[1],P[2]);

  dReal d[3]={0,0,1};
  dReal a[3];
  P=dBodyGetPosition(RWheel->BodyID);
  RR=dBodyGetRotation(RWheel->BodyID);
  RotateVector(RR,d,a);
  dJointSetHingeAxis(RMotor,a[0],a[1],a[2]);
  dJointSetHingeAnchor(RMotor,P[0],P[1],P[2]);

  P=dBodyGetPosition(LWheel->BodyID);
  RR=dBodyGetRotation(LWheel->BodyID);
  d[2]=-1;
  RotateVector(RR,d,a);
  dJointSetHingeAxis(LMotor,a[0],a[1],a[2]);
  dJointSetHingeAnchor(LMotor,P[0],P[1],P[2]);

  SetSpeed(0,0,0.1);

  dJointSetHingeParam(RMotor, dParamFMax, AXLE_FRICTION_TORQUE);
  dJointSetHingeParam(LMotor, dParamFMax, AXLE_FRICTION_TORQUE);

  
  
  dReal pos[3]={0,SCANNER_OFFSET_Y,-SCANNER_OFFSET_Z};
  dMatrix3 rot;
  dRSetIdentity(rot);
  dRFrom2Axes(rot, -1,0,0, 0,1,0);
  
  Children.push_back(LaserScanner=new CURG(World,this,Hull->BodyID,pos,rot,LASER_INTERVAL,&mcs,0));
  //Children.push_back(LaserScanner=new CSick(World,this,Hull->BodyID,pos,rot,LASER_INTERVAL,&mcs,0));
  
  Children.push_back(Sonars=new CSonarSet(World,this,SONAR_INTERVAL,
      SONAR_RANGE,DegToRad(SONAR_CONE_ANGLE),&mcs,1,"Sonars"));
  
  //mid sonar
  dReal sonarpos[3]={0,MID_SONAR_Y-HULL_Y,-SONARS_Z};
  dMatrix3 sonarrot;

  //dRFrom2Axes(sonarrot, 1,0,0, 0,-1,0);
  dRFrom2Axes(sonarrot, 1,0,0, 0,-1,0);
  //dRSetIdentity(sonarrot);
  
  Sonars->AddSonar(Hull->BodyID,sonarpos,sonarrot,0);
    
  //side sonars
  dRFromAxisAndAngle(sonarrot, 0,1,0, DegToRad(180-38.0));
  sonarpos[0]=0.06;
  sonarpos[1]=SIDE_SONAR_Y-HULL_Y;
  sonarpos[2]=-SONARS_Z;
  
  Sonars->AddSonar(Hull->BodyID,sonarpos,sonarrot,DegToRad(-38));
  
  dRFromAxisAndAngle(sonarrot, 0,1,0, DegToRad(180+38.0));
  sonarpos[0]=-0.06;
  sonarpos[1]=SIDE_SONAR_Y-HULL_Y;
  sonarpos[2]=-SONARS_Z;
  
  Sonars->AddSonar(Hull->BodyID,sonarpos,sonarrot,DegToRad(38));
  
}

CMarekbot::~CMarekbot()
{

}

void CMarekbot::Activate()
{
  connectGIMI();
  
  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
  {
    (*i)->Activate();
  }

  MaCI::EMaCIError e=SpeedCtrlServer.Open();
  if (e!=MaCI::KMaCIOK)
  {
    dPrint(1,"Couldnt open SpeedCtrlServer");
  } else {
    SpeedCtrlServer.SetSpeedLimits(MAX_MOTOR_SPEED, MAX_ANGULAR_SPEED, MAX_ACCELERATION,
                                   -MAX_MOTOR_SPEED, -MAX_ANGULAR_SPEED, -MAX_ACCELERATION);
  }
  e=PositionServer.Open();
  if (e!=MaCI::KMaCIOK)
  {
    dPrint(1,"Couldnt open PositionServer");
  }
}

void CMarekbot::Update(float StepSize)
{
  if (SpeedCtrlServer.WaitCommand(SpeedCtrlData,0)==MaCI::KMaCIOK)
  {
    const MaCI::SpeedCtrl::TCommand *c=SpeedCtrlData.GetCommand();
    const MaCI::SpeedCtrl::TSpeed *s;
    if (c)
      switch (c->cmd)
      {
        case MaCI::SpeedCtrl::KCommandSetSpeed:
          s=SpeedCtrlData.GetSpeed();
          if (s)
            SetSpeed(s->speed,s->angularSpeed,s->acceleration);
          break;
        case MaCI::SpeedCtrl::KCommandSetStop:
          SetSpeed(0,0,1);
          break;
        case MaCI::SpeedCtrl::KCommandTimeout:
          SetSpeed(0,0,1);
          break;
      }
  }
  //Send speed event
  MaCI::SpeedCtrl::CSpeedCtrlData speedEvent;
  gim::binbag::CBinBag bag;
  speedEvent.EncodeTo(&bag); 
  speedEvent.SetCommand(MaCI::SpeedCtrl::TCommand(MaCI::SpeedCtrl::KEventSpeed));
  speedEvent.SetSpeed(MaCI::SpeedCtrl::TSpeed(iSpeed,iAngularSpeed));
  //speedEvent.SetTimestamp(MaCI::Common::TTimestamp((int)floorf(t),(int)((t-floorf(t))*1000000)));
  speedEvent.SetTimestamp(MaCI::Common::TTimestamp());
  SpeedCtrlServer.SendSpeedEvent(speedEvent);
  if (positionTimer>=POSITION_INTERVAL)
  {
    const dReal *P, *R;
    dReal v[3]={1,0,0};
    dReal r[3];
    float heading;
    P=dBodyGetPosition(Hull->BodyID);
    R=dBodyGetRotation(Hull->BodyID);
    RotateVector(R,v,r);
#warning "NOTE: Using inverted y-axis and heading"
    heading=atan2(-r[2],r[0])+M_PI/2.0;
    //TODO: fix this heading gludge stuff
    //float t=SimWorld->GetSimulationTime();
    MaCI::Position::CPositionData pd;
    pd.EncodeTo(&binbag);
    //    pd.SetTimestamp(MaCI::Common::TTimestamp((int)floorf(t),(int)((t-floorf(t))*1000000)));
    pd.SetTimestamp(MaCI::Common::TTimestamp());
    //dPrint(1,"odoposx=%f odoposy=%f odoheading=%f", odoPosX, odoPosY, odoHeading);
    pd.SetPose2D(MaCI::Position::TPose2D(P[0],-P[2],heading));
    pd.SetVariance2D(MaCI::Position::TVariance2D(0,0,0));
    pd.SetProbability(MaCI::Position::TProbability(1));

    PositionServer.SendPositionEvent(pd);
    //pd.Print(1);
    positionTimer=0;
  }
  positionTimer+=StepSize;
  
  //update child objects
  //dPrint(1,"Updating children");
  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
  {
    (*i)->Update(StepSize);
  }

}

void CMarekbot::OnCollision(CPrimitive *primitive, int numc, dContact *contacts, CPrimitive *primitive2)
{

}

void CMarekbot::Draw()
{
  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
  {
    (*i)->Draw();
  }

  for (PrimitivesIter i=Parts.begin();i!=Parts.end();i++)
  {
    (*i)->Draw();
  }
}

void CMarekbot::SetSpeed(float speed, float angularSpeed, float acceleration)//, float angularAcceleration)
{
  float leftVelocity = 0.0;
  float rightVelocity = 0.0;
  float ar, al;
  float r = WHEELBASE/2.0;
  float f;
  dMass mass;

  //dPrint(1,"Set speed, speed=%f a.speed=%f acc=%f ",speed,angularSpeed,acceleration);

  rightVelocity = angularSpeed*r+speed;
  leftVelocity = 2.0*speed-rightVelocity;

/*  if (leftVelocity >= -MAX_MOTOR_SPEED && leftVelocity <= MAX_MOTOR_SPEED &&
      rightVelocity >= -MAX_MOTOR_SPEED && rightVelocity <= MAX_MOTOR_SPEED)
  {*/
    ar=rightVelocity/(DRIVING_WHEEL_RADIUS);
    al=-leftVelocity/(DRIVING_WHEEL_RADIUS); //invert this speed
    //dPrint(1,"ar=%f al=%f",ar,al);

    dJointSetHingeParam(this->RMotor, dParamVel, ar);
    dJointSetHingeParam(this->LMotor, dParamVel, al);

    dBodyGetMass(this->Hull->BodyID,&mass);
    f=fabs(mass.mass*acceleration)*DRIVING_WHEEL_RADIUS+1.0;
    //dPrint(1,"mass=%f force=%f",mass.mass,f);

    dJointSetHingeParam (RMotor, dParamFMax, f);
    dJointSetHingeParam (LMotor, dParamFMax, f);
    iSpeed = speed;
    iAngularSpeed = angularSpeed;
    //dPrint(1,"Set speed end");
  /*} else {
    dPrint(1,"Speeds out of range: %f or %f",
	  leftVelocity, rightVelocity);
  }*/
}

void CMarekbot::connectGIMI()
{
  SimWorld->ConnectGimi(&gimi,name);
}

